import rospy
import time
import cv2
import numpy as np
import threading
from threading import Thread
import queue
import module.lowlight_enhancement
from sensor_msgs.msg import Image

#from cv_bridge import CvBridge


#bridge=CvBridge()

class Image_Synchronizer(Thread):
    def __init__(self,imgTopic,imgPub,imgRate):
        super(Image_Synchronizer,self).__init__()
        #self.isAlive=True
        self.Img_Processed=queue.Queue()
        self.ImgBuffer=queue.Queue()
        self.lastUpdate = 0
        self.imgRate=rospy.Rate(20)
        self.imgpub = rospy.Publisher(imgPub, Image, queue_size=2000)
        self.imgsub = rospy.Subscriber(imgTopic, Image, self.handleImage, queue_size=2000)


    def run(self):
        processImage = threading.Thread(target=self.producer)
        publishImage = threading.Thread(target=self.consumer)
        processImage.start()
        time.sleep(160)
        # time.sleep(15)
        print("start sending!")
        publishImage.start()


    def producer(self):
        while True:
            list = []
            Max = 0
            update=True
            current = time.perf_counter()
            if self.ImgBuffer.qsize() > 30:
                Max = 30
            else:
                if self.lastUpdate == 0 or (self.lastUpdate != 0 and current - self.lastUpdate < 9):
                    print("Not enough Image,waiting for new data")
                    time.sleep(3)
                    continue
                else:
                    print("too long for new data")
                    if self.ImgBuffer.qsize() == 0:
                        print("No image received!End imagethis thread")
                        self.isAlive = False
                        return
                    else:
                        update=False
                        Max = self.ImgBuffer.qsize()
            for i in range(0, Max):
                list.append(self.ImgBuffer.get())
            for data in list:
                cv_image= np.frombuffer(data.data, dtype=np.uint8).reshape(data.height, data.width, -1)
                #cv_image = cv2.cvtColor(cv_image, cv2.COLOR_RGB2BGR)
                #cv_image = bridge.imgmsg_to_cv2(data, desired_encoding='bgr8')
                from PIL import Image as I

                frame = I.fromarray(cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB))
                result = module.lowlight_enhancement.lowlight(frame)
                img = cv2.cvtColor(np.asarray(result), cv2.COLOR_RGB2BGR)
                image_message=Image()
                image_message.encoding = 'bgr8'
                image_message.data=np.array(img).tostring()
                image_message.height = img.shape[0]
                image_message.width = img.shape[1]
                image_message.step = img.shape[1] * img.shape[2]
                # image_message = bridge.cv2_to_imgmsg(img, encoding="bgr8")
                image_message.header=data.header
                # self.Img_Processed.put(data)
                self.Img_Processed.put(image_message)
            if update:
                self.lastUpdate=time.perf_counter()
            time.sleep(2)

    def consumer(self):
        while True:
            if self.Img_Processed.qsize() > 0:
                item = self.Img_Processed.get()
                self.imgpub.publish(item)
                self.imgRate.sleep()
            else:
                if self.isAlive:
                    #print("Image send thread is running")
                    time.sleep(3)
                else:
                    print("Image Producer stopped , end this thread too")
                    return
            # print(f"ImageProcessed size :{self.Img_Processed.qsize()}")

    def handleImage(self,data):
        self.putImage(data)

    def putImage(self,data):
        self.ImgBuffer.put(data)

    def getIMU(self):
        self.ImgBuffer.get()